
#include "OpenImu.h"
#include "delay.h"
#include "SpiPeripheral.h"
#include "spi.h"
#include "usart.h"
#include "encoder.h"
#include "usart3.h"
#include "math.h"
#include "timer.h"
#include "kalman.h"
#include "filter.h"
#include <stdbool.h>
#include "usart.h"

static SpiPeripheral spi3(SPI3);
#define STATIC_DRIFT_CNT 100
extern u8 Integr_Flag; //定时器10ms中断标志

/**
* @Axis Z(Yaw), Y(Roll), X(Pitch)
*       Z
*       ^
*       |
*       |    Y
*       |   /
*       |  /
*       | /
*       |/
*       O-------------->X
*/

OpenImu::OpenImu(SpiPeripheral *trans)
{

    Calculus_Time = 0.02000;               //积分时间为20ms，效果最好
    AngleVel_32Bit_Coefficient = 0.015625; //32位角速度系数   单位dg/s/lsb
    Acc_32Bit_Coefficient = 0.00025;       //32位加速度系数   单位 m/s2/lsb
    AngleCoefficient = 0.0054931640625;    //          单位 dg/lsb
    TempCoefficient = 0.1;                 //温度系数

    DriftDataMax = 0.0013333333;  //设置静态偏移量的阈值最大值
    DriftDataMin = -0.0013333333; //设置静态偏移量的阈值最小值

    Movaverage.mv_x.max_cnt = 6; //移动平均滤波队列长度
    Movaverage.mv_x.average = Movaverage.MfDataX;
    Movaverage.mv_y.max_cnt = 6; //移动平均滤波队列长度
    Movaverage.mv_y.average = Movaverage.MfDataY;
    Movaverage.mv_z.max_cnt = 6; //移动平均滤波队列长度
    Movaverage.mv_z.average = Movaverage.MfDataZ;

    Movaverage.mv_zaxis.max_cnt = 6;
    Movaverage.mv_zaxis.average = Movaverage.MfDataMV;
    _ptrans = trans;
}

OpenImu *OpenImu::get(void)
{
    static OpenImu OpenImu_(&spi3);
    return &OpenImu_;
}

u16 OpenImu::writeRegister(uint16_t pBuffer, u16 RegisterAddr)
{
    u16 data[2] = {0, 0};
    data[0] = pBuffer & 0x00ff;
    u16 ret = 0;

    _ptrans->chipSelect();
    delay_us(6);
    _ptrans->readAfterWrite(((RegisterAddr << 8) | 0x8000) | data[0]);
    delay_us(1);
    _ptrans->chipRelease();
    delay_us(10);

    return ret;
}

u16 OpenImu::readRegister(u16 regaddr)
{
    u16 ret[2] = {0, 0};
    u16 data = 0x0000;
    delay_us(16);
    _ptrans->chipSelect();
    delay_us(16);
    ret[0] = _ptrans->readAfterWrite((uint16_t)((regaddr << 8)));

    //    _ptrans->chipRelease();
    delay_us(16);
    //    _ptrans->chipSelect();
    ret[1] = _ptrans->readAfterWrite(0x0000);
    delay_us(16);
    _ptrans->chipRelease();
    data = ret[1];
    ret[1] = 0;
    ret[0] = 0;
    return data;
}

ImuStatusTypeDef OpenImu::BurstReadRegister(u16 RegisterAddr)
{
    u8 DataNumber = 0;

    if (RegisterAddr == OpenImu_BURST)
    {
        DataNumber = 8;
    }

    if (RegisterAddr == OpenImu_BURST_VG)
    {
        DataNumber = 11;
    }

    if (RegisterAddr == OpenImu_BURST_MAG)
    {
        DataNumber = 11;
    }

    RegisterAddr = RegisterAddr << 8; //左移8位得到寄存器16位地址

    delay_us(200);
    _ptrans->chipSelect();
    delay_us(100);
    _ptrans->readAfterWrite(RegisterAddr);
    //     delay_us(1);
    for (u8 i = 0; i < DataNumber; i++) //连续写DataNumber个0x00
    {
        delay_us(50);
        OpenImu_Data.Original.Burst32_Data[i] = _ptrans->readAfterWrite(0x0000);
    }
    delay_us(100);
    _ptrans->chipRelease();
    delay_us(200);
    return Imu_STATUS_OK;
}

u8 OpenImu::GetData()
{
    OpenImu_Data.X_Axis.AngelVel = OpenImu_Data.Original.Burst32_Data[1];
    OpenImu_Data.Y_Axis.AngelVel = OpenImu_Data.Original.Burst32_Data[2];
    OpenImu_Data.Z_Axis.AngelVel = OpenImu_Data.Original.Burst32_Data[3];
    OpenImu_Data.X_Axis.LineAcc = OpenImu_Data.Original.Burst32_Data[4];
    OpenImu_Data.Y_Axis.LineAcc = OpenImu_Data.Original.Burst32_Data[5];
    OpenImu_Data.Z_Axis.LineAcc = OpenImu_Data.Original.Burst32_Data[6];
    OpenImu_Data.temperature = OpenImu_Data.Original.Burst32_Data[7];
    //    OpenImu_Data.X_Axis.Angle   =OpenImu_Data.Original.Burst32_Data[8];        //Roll
    //    OpenImu_Data.Y_Axis.Angle   =OpenImu_Data.Original.Burst32_Data[9];        //Pitch
    //    OpenImu_Data.Z_Axis.Angle   =OpenImu_Data.Original.Burst32_Data[10];       //Yaw

    return 1;
}

u8 OpenImu ::SetMode(bool mode)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE); //使能PORTA,PORTE时钟

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;         //KEY0-KEY1
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
    if (mode == true)                                 //串口模式，上电拉低该引脚DR
    {
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //设置成上拉输入
        GPIO_Init(GPIOD, &GPIO_InitStructure);           //初始化GPIOE4,3
        GPIO_ResetBits(GPIOD, GPIO_Pin_4);               //PB.5 输出高
    }
    else
    {
        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //设置成上拉输入
        GPIO_Init(GPIOD, &GPIO_InitStructure);                //初始化GPIOE4,3
    }
    return mode;
}

void OpenImu ::ImuResestInit()
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE); //使能PD端口时钟

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;         //LED0-->PB.5 端口配置
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;  //推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度为50MHz
    GPIO_Init(GPIOD, &GPIO_InitStructure);            //根据设定参数初始化GPIOB.5
    GPIO_SetBits(GPIOD, GPIO_Pin_6);                  //PB.5 输出高
}

void OpenImu ::ImuResest()
{
    GPIO_SetBits(GPIOD, GPIO_Pin_6); //PB.5 输出高
    delay_ms(5);
    GPIO_ResetBits(GPIOD, GPIO_Pin_6); //PB.5 输出高
    delay_ms(5);
    GPIO_SetBits(GPIOD, GPIO_Pin_6); //PB.5 输出低
}

u16 OpenImu::getDeviceId(void)
{
    u16 ret = 0;
    ret = readRegister(OpenImu_PROD_ID_HHIGH);
    OpenImu_Data.Imu_Id = ret;
    return ret;
}

bool OpenImu::init(void)
{

    //    ImuResestInit();

    //    SetMode(false);
    //
    //    GPIO_ResetBits(GPIOD,GPIO_Pin_6);             //PB.5 输出高
    //
    //
    //    _ptrans->begin();
    //
    //    GPIO_SetBits(GPIOD,GPIO_Pin_6);               //PB.5 输出低

    SetMode(true); //启动串口模式
    delay_ms(500);
    while (ImuCalibration_Uart(0x64) != Imu_STATUS_OK) //Imu校准数据，获得静态漂移量      2000可以   0x1f4
    {
        //       delay_ms(1);
    }

    return true;
}

ImuStatusTypeDef OpenImu ::ImuGet_Drift(int *Bit32_Drift) //Imu获取数据静态漂移量
{
    if (Drinft_1S_flag == 1)
    {
        if (Drinft_1S_cnt <= 100)
        {
            Drinft_1S_cnt++;
            return Imu_STATUS_ERROR;
        }
        else if ((Drinft_1S_cnt > 50) && (Drinft_1S_cnt <= 150))
        {
            OpenImu_Data.Drift_Data.Bit32_Mode.Drinft += Bit32_Drift[3];

            Drinft_1S_cnt++;
            return Imu_STATUS_ERROR;
        }
        else if (Drinft_1S_cnt == 151)
        {
            OpenImu_Data.Drift_Data.Bit32_Mode.Drinft = -(OpenImu_Data.Drift_Data.Bit32_Mode.Drinft / 100.0f);
            OpenImu_Data.Drift_Data.Bit32_Mode.Original__Data = OpenImu_Data.Drift_Data.Bit32_Mode.Drinft * AngleVel_32Bit_Coefficient;
            Drinft_1S_cnt = 153;
            return Imu_STATUS_OK;
        }
        else
            return Imu_STATUS_OK;
    }
    else
        return Imu_STATUS_ERROR;
}

float OpenImu ::ImuUpdateDrift()
{
    if (DriftTask.task_flag == true) //判断wingbot是否静止
    {
        if (ImuFlag.calibration == true)
        {
            if (DriftTask.time_flag == true)
            {
                if (Movaverage.mv_z.anglevel >= OpenImu_Data.wingbotDrift.max)
                {
                    OpenImu_Data.wingbotDrift.max = Movaverage.mv_z.anglevel; //拿到一个循环中的最大极值
                }
                if (Movaverage.mv_z.anglevel <= OpenImu_Data.wingbotDrift.min)
                {
                    OpenImu_Data.wingbotDrift.min = Movaverage.mv_z.anglevel; //拿到一个循环中的最小极值
                }

                OpenImu_Data.wingbotDrift.filteranglevel = Movaverage.mv_z.anglevel;
                OpenImu_Data.wingbotDrift.sum += Movaverage.mv_z.anglevel; //15ms中断;
                OpenImu_Data.wingbotDrift.sum_cnt++;                       //偏移累加量+1
                                                                           //  DriftTask.task_sum_cnt++;                              //进入中断次数+1
                if (OpenImu_Data.wingbotDrift.sum_cnt == 1000)             //如果15s钟时间到，15ms中断，进入1000次为15s
                {
                    //去掉极大，极小值
                    OpenImu_Data.wingbotDrift.sum = OpenImu_Data.wingbotDrift.sum - OpenImu_Data.wingbotDrift.max - OpenImu_Data.wingbotDrift.min;
                    //计算去掉极值后998个点的均值作为偏移量
                    OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = -(OpenImu_Data.wingbotDrift.sum / (1000.0f - 2.0f)); //更新一次偏移量
                                                                                                                         //把当前偏移量保存到上次
                    OpenImu_Data.Drift_Data.Bit32_Mode.PreviousTrueDrift = OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift;

                    OpenImu_Data.wingbotDrift.max = 0; //清空极大值
                    OpenImu_Data.wingbotDrift.min = 0; //清空极小值

                    OpenImu_Data.wingbotDrift.sum_cnt = 0;
                    OpenImu_Data.wingbotDrift.sum = 0;
                    DriftTask.task_sum_cnt = 0;
                }
                else //如果1s钟时间未到
                {
                    if (DriftTask.task_flag == false) //判断wingbot是否静止
                    {
                        //如果开始运动，则结算当前时间偏移量
                        // OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift=-OpenImu_Data.wingbotDrift.sum/(OpenImu_Data.wingbotDrift.sum_cnt*0.01);     //更新一次偏移量
                        // OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift=-OpenImu_Data.wingbotDrift.sum/(OpenImu_Data.wingbotDrift.sum_cnt);     //更新一次偏移量
                        // OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift=OpenImu_Data.Drift_Data.Bit32_Mode.PreviousTrueDrift;
                        OpenImu_Data.wingbotDrift.max = 0;     //清空极大值
                        OpenImu_Data.wingbotDrift.min = 0;     //清空极小值
                        OpenImu_Data.wingbotDrift.sum_cnt = 0; //清空累加次数
                        OpenImu_Data.wingbotDrift.sum = 0;     //清空当前一个循环内偏移的累加值
                        DriftTask.task_sum_cnt = 0;
                    }
                }
                DriftTask.time_flag = false; //ms中断清除
            }
            return OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift;
        }
        else
        {
            return 0;
        }
    }
    else
    {
        return 0;
    }
}

u8 OpenImu ::getWingbotVel(u8 *hostdata)
{
    if (hostdata[7] == 1) // 整车速度为0
    {
        if (ImuFlag.calibration == true) //只有当校准完之后才开启定时器中断
        {
            wingbot.wingbot_vel = 1;    //则该位置1，表示整车静止
            DriftTask.task_flag = true; //开启定时器2的中断
                                        //        TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE ); //使能指定的TIM3中断,允许更新中断
                                        //        TIM_Cmd(TIM2, ENABLE);  //使能定时器2
        }
    }
    else
    {
        wingbot.wingbot_vel = 0;     //则该位置0，表示整车运动
        DriftTask.task_flag = false; //关闭定时器2的中断
                                     //      TIM_ITConfig(TIM2,TIM_IT_Update,DISABLE ); //使能指定的TIM3中断,允许更新中断
                                     //      TIM_Cmd(TIM2, DISABLE);  //使能定时器2
    }
    return 0;
}

float OpenImu ::ImuGetTemperature() //Imu得到Z轴角速度
{
    s16 ret = 0;
    float ret1 = 0;

    ret = readRegister(OpenImu_Sensor_TEMP_HIGH); //读取温度
    ret1 = ret * 0.073111172849435 + 31.0;        //温度*敏感度
    OpenImu_Data.temperature = ret1;
    return ret1;
}
float OpenImu ::ImuGetTempDrift() //Imu得到温度漂移
{
    float temp = 0;
    temp = ImuGetTemperature();
    return temp;
}

float OpenImu ::ImuGetZaxisAnglevel(bool type) //Imu得到Z轴角速度
{
    s16 ret = 0;
    s16 ret4 = 0;
    float ret5 = 0;
    ret = this->readRegister(OpenImu_Z_GYRO_HIGH); //发送读数据指令,得到角速度
    this->OpenImu_Data.Original.Burst32_Data[3] = ret;

    ret4 = ret;
    ret5 = ret * this->AngleVel_32Bit_Coefficient;

    //   Movaverage.mv_x.input= ret;             //将原始整数数据导入滤波结构体
    //    //传入滤波结构体指针，进行移动平均滤波  原始数据*系数算出原始角速度;
    //    Movaverage.mv_x.anglevel=AntiPulse_MovingAverage_Filter32(&Movaverage.mv_x)* this->AngleVel_32Bit_Coefficient;
    if (type == true)
    {
        return ret5;
    }
    else
    {
        return ret4;
    }
}

u8 OpenImu ::GetExtreme(float *data)
{
    if (*data <= OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[0]) //最小值
    {
        OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[0] = OpenImu_Data.Z_Axis.AngelVel; //  取最小值
    }
    if (*data >= OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[1]) //最大值
    {
        OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[1] = OpenImu_Data.Z_Axis.AngelVel; //   取最大值
    }
    return 1;
}

ImuStatusTypeDef OpenImu ::UartGetAngleVel(Queue *queue) //Imu校准数据，获得静态漂移量
{
    unsigned int f = 0;
    OpenImu_Data.ImuTime.PreviuosTime = OpenImu_Data.ImuTime.NowTime;

    f = (unsigned int)queue->Data[5] + ((unsigned int)queue->Data[6] << 8) + ((unsigned int)queue->Data[7] << 16) + ((unsigned int)queue->Data[8] << 24);
    unsigned int *g = NULL;
    g = (unsigned int *)(&f);
    OpenImu_Data.ImuTime.SysTimeMsec = *g;

    OpenImu_Data.ImuTime.NowTime = OpenImu_Data.ImuTime.SysTimeMsec;

    int d = (queue->Data[29] + (queue->Data[30] << 8) + (queue->Data[31] << 16) + (queue->Data[32] << 24));
    float *e = NULL;
    e = (float *)(&d);
    OpenImu_Data.Z_Axis.AngelVel = - (*e);
    //       float a=*e;
    //       OpenImu_Data.Z_Axis.AngelVel=-a;

    return Imu_STATUS_OK;
}

ImuStatusTypeDef OpenImu ::ImuCalibration_Uart(int times) //Imu校准数据，获得静态漂移量
{
    Queue_Extract(&ImuUart4);               //从队列中提取一帧数据  ，并更新一次 usart3_Receive_Data[]处理数组
                                            //
    if (ImuUart4.ProceseeReceiveQueue != 0) //如果待处理进程不为0
    {

        UartGetAngleVel(&ImuUart4);

        if (OpenImu_Data.Drift_Data.cnt < times) //    #前100次累积静态偏移量
        {
            GetExtreme(&OpenImu_Data.Z_Axis.AngelVel);
            OpenImu_Data.Drift_Data.InitDrift += OpenImu_Data.Z_Axis.AngelVel; //      #静态偏移量累加

            OpenImu_Data.Drift_Data.cnt++; //      #累加计数
        }
        if (OpenImu_Data.Drift_Data.cnt == times) //    #前100次累积静态偏移量
        {
            OpenImu_Data.Drift_Data.InitDrift = OpenImu_Data.Drift_Data.InitDrift - OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[0] - OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[1];
            OpenImu_Data.Drift_Data.InitDrift = -OpenImu_Data.Drift_Data.InitDrift / (times - 2.0); //      #静态偏移量累加
            OpenImu_Data.Drift_Data.cnt++;                                                          //      #累加计数
            OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[0] = 0.0;
            OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[1] = 0.0;
        }
        if ((OpenImu_Data.Drift_Data.cnt >= times) && (OpenImu_Data.Drift_Data.cnt <= 2 * times)) //    #前100次累积静态偏移量
        {

            OpenImu_Data.Z_Axis.AngelVel = OpenImu_Data.Z_Axis.AngelVel + OpenImu_Data.Drift_Data.InitDrift; //      #静态偏移量累加

            GetExtreme(&OpenImu_Data.Z_Axis.AngelVel);

            OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift += OpenImu_Data.Z_Axis.AngelVel; //      #静态偏移量累加
            OpenImu_Data.Drift_Data.cnt++;
        }

        if (OpenImu_Data.Drift_Data.cnt == 2 * times) //    #前100次累积静态偏移量
        {
            OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift - OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[0] - OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[1];

            OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = -OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift / (times - 2.0); //      #静态偏移量累加
            OpenImu_Data.Drift_Data.cnt++;                                                                                  //      #累加计数

            //         float a=-OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift-OpenImu_Data.Drift_Data.InitDrift;
            if (ImuCheckCalibration(&OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift)) //校验静态偏移量是否超过阈值不合理
            {
                OpenImu_Data.Drift_Data.cnt = 0;
                OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[0] = 0.0;
                OpenImu_Data.Drift_Data.Bit32_Mode.ExtremeFloat[1] = 0.0;

                static u8 i = 0;
                static float Drift[10] = {0.0};
                OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift + OpenImu_Data.Drift_Data.InitDrift;
                Drift[i] = OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift;
                OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = 0.0;
                OpenImu_Data.Drift_Data.InitDrift = 0.0;
                i++;
                if (i == 10)
                {
                    float angvel = 0.0;
                    for (u8 a = 0; a < 10; a++)
                    {
                        angvel += Drift[a];
                    }
                    OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = angvel / 10.0f;
                    ImuFlag.calibration = true; //更新校准标志
                    return Imu_STATUS_OK;
                }
                return Imu_STATUS_ERROR;
            }
            else
            {
                OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift + OpenImu_Data.Drift_Data.InitDrift;
                ImuFlag.calibration = true; //更新校准标志
                return Imu_STATUS_OK;
            }
        }
    }
    return Imu_STATUS_ERROR;
}

float OpenImu ::ImuUpdateDriftUart(float *vel)
{
    if (DriftTask.task_flag == true) //判断wingbot是否静止
    {
        if (ImuFlag.calibration == true)
        {
            if (DriftTask.time_flag == true)
            {
                if (*vel >= OpenImu_Data.wingbotDrift.max)
                {
                    OpenImu_Data.wingbotDrift.max = *vel; //拿到一个循环中的最大极值
                }
                if (*vel <= OpenImu_Data.wingbotDrift.min)
                {
                    OpenImu_Data.wingbotDrift.min = *vel; //拿到一个循环中的最小极值
                }

                OpenImu_Data.wingbotDrift.filteranglevel = *vel;
                OpenImu_Data.wingbotDrift.sum += *vel;         //15ms中断;
                OpenImu_Data.wingbotDrift.sum_cnt++;           //偏移累加量+1
                if (OpenImu_Data.wingbotDrift.sum_cnt == 1000) //如果15s钟时间到，15ms中断，进入1000次为15s
                {
                    //去掉极大，极小值
                    OpenImu_Data.wingbotDrift.sum = OpenImu_Data.wingbotDrift.sum - OpenImu_Data.wingbotDrift.max - OpenImu_Data.wingbotDrift.min;
                    //计算去掉极值后998个点的均值作为偏移量
                    OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = -(OpenImu_Data.wingbotDrift.sum / (1000.0f - 2.0f)); //更新一次偏移量
                                                                                                                         //把当前偏移量保存到上次
                    OpenImu_Data.Drift_Data.Bit32_Mode.PreviousTrueDrift = OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift;

                    OpenImu_Data.wingbotDrift.max = 0; //清空极大值
                    OpenImu_Data.wingbotDrift.min = 0; //清空极小值

                    OpenImu_Data.wingbotDrift.sum_cnt = 0;
                    OpenImu_Data.wingbotDrift.sum = 0;
                    DriftTask.task_sum_cnt = 0;
                }
                else //如果1s钟时间未到
                {
                    if (DriftTask.task_flag == false) //判断wingbot是否静止
                    {
                        //如果开始运动，则结算当前时间偏移量
                        OpenImu_Data.wingbotDrift.max = 0;     //清空极大值
                        OpenImu_Data.wingbotDrift.min = 0;     //清空极小值
                        OpenImu_Data.wingbotDrift.sum_cnt = 0; //清空累加次数
                        OpenImu_Data.wingbotDrift.sum = 0;     //清空当前一个循环内偏移的累加值
                        DriftTask.task_sum_cnt = 0;
                    }
                }
                DriftTask.time_flag = false; //ms中断清除
            }
            return OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift;
        }
        else
        {
            return 0;
        }
    }
    else
    {
        return 0;
    }
}


u8 OpenImu ::UartanalysisImuData()
{
    Queue_Extract(&ImuUart4); //从队列中提取一帧数据  ，并更新一次 usart3_Receive_Data[]处理数组

    if (ImuUart4.ProceseeReceiveQueue != 0) //如果待处理进程不为0
    {
        UartGetAngleVel(&ImuUart4);
        ImuUpdateDriftUart(&OpenImu_Data.Z_Axis.AngelVel); //静止状态下更新偏移量，动态下使用上一次偏移量
        OpenImu_Data.ImuTime.DeltaTime = (float)(OpenImu_Data.ImuTime.NowTime - OpenImu_Data.ImuTime.PreviuosTime);
        OpenImu_Data.ImuTime.DeltaTime = OpenImu_Data.ImuTime.DeltaTime / 1000000.0;
        OpenImu_Data.Z_Axis.AngelVel = OpenImu_Data.Z_Axis.AngelVel + OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift; //原始数据加上偏移值
    

        for (u8 i = 0; i < ImuUart4.FrameLength; i++)
        {
            ImuUart4.Data[i] = 0x00; //处理完一条指令，进程数组清零
        }
    }
    else
        return 0;

    return 1;
}
/**
 * @brief  Set ADI Accelerometer output data rate when enabled
 * @param  Bit32_Drift: 传入的原始数据
 * @param  times:       要测量的次数,初定为5000个点
 * @retval Imu_STATUS_OK in case of success, an error code otherwise
 */
ImuStatusTypeDef OpenImu ::ImuCalibration(int times) //Imu校准数据，获得静态漂移量
{

    float av = 0;
    float drift_sum[2] = {0u};     //偏移量的累加和
    float drift_average[2] = {0u}; //偏移量的均值

    for (int i = 0; i < times; i++) //循环累加times次，得到累积上电偏移值
    {
        delay_ms(1);
        av = ImuGetZaxisAnglevel(true);
        if (av <= OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[0]) //最小值
        {
            OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[0] = av; //  取最小值
        }
        if (av >= OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[1]) //最大值
        {
            OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[1] = av; //   取最大值
        }
        drift_sum[0] += av;
    }

    //去掉两个最大最小值，剩下的点取平均
    drift_sum[0] = drift_sum[0] - OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[1] - OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[0];
    drift_average[0] = -(drift_sum[0] / (times - 2)); //得到偏移量累加量的均值

    OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = drift_average[0];

    for (int i = 0; i < times; i++) //循环累加times次，得到累积上电偏移值
    {
        delay_ms(1); //100
        av = ImuGetZaxisAnglevel(true) + OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift;
        OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[0] = 0;
        OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[1] = 0;
        if (av <= OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[0]) //最小值
        {
            OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[0] = av; //  取最小值
        }
        if (av >= OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[1]) //最大值
        {
            OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[1] = av; //   取最大值
        }
        drift_sum[1] += av;
    }

    drift_sum[1] = drift_sum[1] - OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[1] - OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[0];
    drift_average[1] = (drift_sum[1] / (times - 2)); //得到偏移量累加量的均值

    OpenImu_Data.Drift_Data.Bit32_Mode.Drinft = -drift_average[1]; //更新静态偏移量到结构体
    OpenImu_Data.Drift_Data.Bit32_Mode.Original__Data = -drift_average[1];
    //OpenImu_Data.Drift_Data.Bit32_Mode.Original__Data= (-drift_average[1]+drift_average[0])/2.0f;

    /**********清零相关数据，防止校验失败上次结果影响********/
    OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[0] = 0;
    OpenImu_Data.Drift_Data.Bit32_Mode.Extreme_Value[1] = 0;
    /********************************************************/
    ImuGetTempDrift();                                                           //得到温漂
    if (ImuCheckCalibration(&OpenImu_Data.Drift_Data.Bit32_Mode.Original__Data)) //校验静态偏移量是否超过阈值不合理
    {
        return Imu_STATUS_ERROR;
    }
    else
    {
        OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift = OpenImu_Data.Drift_Data.Bit32_Mode.Original__Data;
        ImuFlag.calibration = true; //更新校准标志
        return Imu_STATUS_OK;
    }
}

ImuStatusTypeDef OpenImu ::ImuCheckCalibration(float *drift) //Imu获取到静态偏移量后检查偏移量是否正确
{
    if (*drift > DriftDataMax)
    {
        ImuFlag.calibration = false; //更新校准标志
        return Imu_STATUS_ERROR;
    }
    else if (*drift < DriftDataMin)
    {
        ImuFlag.calibration = false; //更新校准标志
        return Imu_STATUS_ERROR;
    }
    else
    {
        ImuFlag.calibration = false;
        return Imu_STATUS_OK;
    }
}

ImuStatusTypeDef OpenImu ::ImuAcc_32Bit_DataHandle(u16 *OriginalData) //原始32位数据的值进行转化
{
    s16 Original = 0;
    Original = (s16)OriginalData[3];
    //OpenImu_Data.Original.anglevel=Original* this->AngleVel_32Bit_Coefficient;  //  原始数据*系数算出原始角速度;

    //对原始角速度做一阶低通滤波
    const float factor = 0.001f; //滤波因素
    static float tBuff;
    tBuff = tBuff * (1 - factor) + (float)Original * factor;
    OpenImu_Data.Original.anglevel = tBuff * AngleVel_32Bit_Coefficient; //  原始数据*系数算出原始角速度

    //IMU  Z轴角速度进行积分
    if (Integr_Flag == 1) // Integr_Flag=1表示定时器15ms中断已到进行积分
    {
        if (ImuFlag.calibration == true) //Imu获取数据静态漂移量
        {

            //一重积分算出线速度和角度
            //            this->OpenImu_Data.Z_Axis.AngelVel = OpenImu_Data.Original.anglevel+OpenImu_Data.Drift_Data.Bit32_Mode.True_Drift;
            this->OpenImu_Data.Z_Axis.AngelVel = OpenImu_Data.Original.anglevel;
            //            this->OpenImu_Data.Z_Axis.AngelVel= this->ImuGetZaxisAnglevel(true);
            this->OpenImu_Data.Z_Axis.Angle += this->OpenImu_Data.Z_Axis.AngelVel * Calculus_Time;
            //              this->OpenImu_Data.Z_Axis.Angle+=this->OpenImu_Data.Z_Axis.AngelVel*( ImuTime.nowtime- ImuTime.previous_time)/1000.0f;
            //              ImuTime.previous_time= ImuTime.nowtime;
            //一重低通滤波角度

            //             ImuUpdateDrift();
        }
        else
            this->OpenImu_Data.Z_Axis.Angle = 0;

        Integr_Flag = 0; //清零定时器标志位标志位
    }
    return Imu_STATUS_OK;
}

ImuStatusTypeDef OpenImu::ImuAccDataConversion(u16 *OriginalData) //IMU
{
    return Imu_STATUS_OK;
}

ImuStatusTypeDef OpenImu ::ImuData_Clear() //Imu数据清零
{
    this->OpenImu_Data.Z_Axis.AngelVel = 0;
    this->OpenImu_Data.Z_Axis.Angle = 0;
    Movaverage.mv_z.angl = 0;
    LowpassFilter.Z_Axis.Angle = 0;
    this->OpenImu_Data.F4_data.Offest_Data = 0;
    Zero_Status[4] = 0x01; //更新IMU清零的状态，1表示清零成功
                           //   memset(this->OpenImu_Data.Z_Axis, 0, sizeof( OpenImu_Data.Z_Axis));
    return Imu_STATUS_OK;
}

//IMU通过串口返回F4实时位置和角度数据的函数  40个字节
ImuStatusTypeDef OpenImu ::OpenImu_Return_F4_PositionData(u8 *order)
{

    u8 sum = 0;
    u8 i = 0;
    float *fAngle;
    OpenImu_Data.Original.ImuSendReturnData[0] = 0x0d;
    OpenImu_Data.Original.ImuSendReturnData[1] = 0x0a;
    //判断是否为修正角度命令(0x06)或者返回实时位置命令（0x04）
    if ((order[0] == 0x0d) && (order[1] == 0x0a) && (order[2] == 0x06) && (order[OpenImu_RX_data_len - 2] == 0x5a) && (order[OpenImu_RX_data_len - 1] == 0xa5))
    {
        OpenImu_Data.Original.ImuSendReturnData[2] = 0x06;
    }
    else
    {
        OpenImu_Data.Original.ImuSendReturnData[2] = 0x04;
    }
    for (i = 0; i < 4; i++)
    {
        OpenImu_Data.Original.ImuSendReturnData[3 + i] = 0x00; //1号IMU数据为0（LSM6DSLTR）
    }

    /***************填充2号IMU数据将float型原始数据*1000转换为整型，拆分为4个字节放入返回数组*********/

    fAngle = (float *)&OpenImu_Data.Original.ImuSendReturnData[7];
    *fAngle = OpenImu_Data.Z_Axis.Angle;

    /***************************************************************************************************/
    for (i = 0; i < 2; i++)
    {
        for (u8 j = 0; j < 4; j++)
        {
            OpenImu_Data.Original.ImuSendReturnData[11 + i * 4 + j] = 0x00; //其余预留位填充为0
        }
    }
    OpenImu_Data.Original.ImuSendReturnData[OpenImu_RX_data_len - 4] = 0x00;
    OpenImu_Data.Original.ImuSendReturnData[OpenImu_RX_data_len - 5] = 0x00;

    for (i = 0; i < (OpenImu_RX_data_len - 5); i++)
        sum += OpenImu_Data.Original.ImuSendReturnData[2 + i]; //CRC校验，把除（包头包尾和校验位）的所有数据累加
    OpenImu_Data.Original.ImuSendReturnData[OpenImu_RX_data_len - 3] = sum;
    OpenImu_Data.Original.ImuSendReturnData[OpenImu_RX_data_len - 2] = 0x5a;
    OpenImu_Data.Original.ImuSendReturnData[OpenImu_RX_data_len - 1] = 0xa5;

    UART3_Send_Array(OpenImu_Data.Original.ImuSendReturnData, OpenImu_RX_data_len);

    Frame_Sign_usart3.now_status = Usart_Initial_number;
    // printf("发送成功\r\n");
    // for(i=0;i<OpenImu_Returndata_len;i++)
    // printf("OpenImu_Send_Return_data[%d]:%d\r\n",i,OpenImu_Send_Return_data[i]);

    // printf("%x",OpenImu_Send_Return_data[i]);
    // printf("OpenImu_Send_Return_data[%d]:%d\r\n",i,OpenImu_Send_Return_data[i]);
    // printf("发送成功\r\n");

    return Imu_STATUS_OK;
}

//IMU通过串口返回F4实时位置角度清零是否成功的函数    12个字节
ImuStatusTypeDef OpenImu ::OpenImu_Return_F4_ZeroData()
{
    u8 sum = 0;
    u8 i = 0;

    OpenImu_Data.Original.ImuSendReturnData[0] = 0x0d;
    OpenImu_Data.Original.ImuSendReturnData[1] = 0x0a;
    OpenImu_Data.Original.ImuSendReturnData[2] = 0x02;
    for (i = 0; i < 2; i++)
    {
        OpenImu_Data.Original.ImuSendReturnData[3 + i] = this->OpenImu_Data.ZeroState.Zero_Flag[i];
    }
    for (i = 0; i < 4; i++)
    {
        OpenImu_Data.Original.ImuSendReturnData[5 + i] = 0x00;
    }

    for (i = 0; i < 7; i++)
        sum += Send_Retutn_data[2 + i];
    OpenImu_Data.Original.ImuSendReturnData[9] = sum;
    OpenImu_Data.Original.ImuSendReturnData[10] = 0x5a;
    OpenImu_Data.Original.ImuSendReturnData[11] = 0xa5;

    UART3_Send_Array(OpenImu_Data.Original.ImuSendReturnData, 12);
    for (i = 0; i < 2; i++)
    {
        this->OpenImu_Data.ZeroState.Zero_Flag[i] = 0x00;
    }
    Frame_Sign_usart3.now_status = Usart_Initial_number; //代表一帧发送结束

    return Imu_STATUS_OK;
}

ImuStatusTypeDef OpenImu ::OpenImu_Analysis_F4_Order(u8 *order) //在解析到F4下发的指令后解析指令的具体内容,参数为F4下发的串口数据
{

    if (order[2] == F4_Angle_Imu_Order)
    {
        if (order[4] == 1)
            OpenImu_Return_F4_PositionData(usart3_Receive_Data);
        else
            ;
    }
    else if (order[2] == F4_Zero_Imu_Order)
    {
        if (order[4] == 1)
            OpenImu_Return_F4_ZeroData();
        else
            ;
    }
    else if (order[2] == F4_OffestAngle_Imu_Order)
    {
        if (order[4] == 1)
        {
            OpenImu_F4_Offest_Position(usart3_Receive_Data);
            OpenImu_Return_F4_PositionData(usart3_Receive_Data);
        }
        else
            ;
    }
    else
    {
        ;
    }
    Frame_Sign_usart3.now_status = Usart_Initial_number; //串口指令处理完的清零标志
    return Imu_STATUS_OK;
}

ImuStatusTypeDef OpenImu ::OpenImu_F4_Offest_Position(u8 *order) //在解析到F4下发的指令后,执行F4下发的校准指令，内容位修正的角度位置
{
    int F4_offest = 0;
    float offest = 0.0;
    for (u8 i = 0; i < 4; i++)
    {
        F4_offest += (u32)order[5 + i] << (i * 8);
    }
    offest = float(F4_offest / 1000.0f);
    this->OpenImu_Data.Z_Axis.Angle = this->OpenImu_Data.Z_Axis.Angle - offest;
    Frame_Sign_usart3.now_status = Usart_Initial_number;
    return Imu_STATUS_OK;
}
